import CGAL
import numpy as np
from CGAL.CGAL_Kernel import Point_3
from CGAL.CGAL_Point_set_3 import Point_set_3
from CGAL.CGAL_Advancing_front_surface_reconstruction import Advancing_front_surface_reconstruction

# Load the point cloud from a PCD file using Open3D
import open3d as o3d
point_cloud = o3d.io.read_point_cloud("./reconstruction/yebo/test/03-03-00.pcd")

# Convert the point cloud to a CGAL point set
points = [Point_3(p[0], p[1], p[2]) for p in np.asarray(point_cloud.points)]
point_set = Point_set_3(points)

# Set the reconstruction parameters
params = Advancing_front_surface_reconstruction.Parameters()
params.facet_angle = 25
params.facet_size = 0.05
params.facet_distance = 0.02
params.cell_radius_edge_ratio = 2
params.cell_size = 0.1

# Perform the surface reconstruction
reconstruction = Advancing_front_surface_reconstruction(point_set, params)
reconstruction.reconstruct_surface()

# Get the output mesh
mesh = reconstruction.surface_mesh()

# Export the mesh as a PLY file
CGAL.write_polygon_mesh_to_file(mesh, "output.ply")